/******************************************************************************
 * Copyright 2018 The Apollo Authors. All Rights Reserved.
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 * http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 *****************************************************************************/

#include "modules/canbus/vehicle/agilex/protocol/accel_cmd_130.h"

#include "modules/drivers/canbus/common/byte.h"

namespace apollo {
namespace canbus {
namespace agilex {

using ::apollo::drivers::canbus::Byte;

const int32_t Accelcmd130::ID = 0x130;

// public
Accelcmd130::Accelcmd130() { Reset(); }

uint32_t Accelcmd130::GetPeriod() const {
  // TODO(QiL) :modify every protocol's period manually
  static const uint32_t PERIOD = 20 * 1000;
  return PERIOD;
}

void Accelcmd130::UpdateData(uint8_t* data) {
  set_p_accel_cmd(data, accel_cmd_);
  set_p_steer_cmd(data, steer_cmd_);
  set_p_option_cmd(data, option_cmd_);
  set_p_loop_cmd(data, loop_cmd_++);
  calc_p_checksum(data);
}

void Accelcmd130::Reset() {
  // TODO(QiL) :you should check this manually
  accel_cmd_ = 0.0;
  option_cmd_ = 1;
  steer_cmd_ = 0.0;
  loop_cmd_ = 0;
}

Accelcmd130* Accelcmd130::set_accel_cmd(double accel_cmd) {
  accel_cmd_ = accel_cmd;

  return this;
}

Accelcmd130* Accelcmd130::set_steer_cmd(double steer_cmd)
{
  steer_cmd_ = steer_cmd;
  return this;
}

Accelcmd130* Accelcmd130::set_option_cmd(uint32_t option_cmd)
{
  option_cmd_ = option_cmd;
  return this;
}

void Accelcmd130::set_p_accel_cmd(uint8_t* data, double accel_cmd) {
  if (accel_cmd > 100.) accel_cmd = 100.;
  int x = accel_cmd;
  uint8_t t = 0;

  t = x & 0xFF;
  Byte to_set0(data + 2);
  to_set0.set_value(t, 0, 8);
}

void Accelcmd130::set_p_steer_cmd(uint8_t* data, double steer_cmd) {
  if (steer_cmd > 100.) steer_cmd = 100.;
  if (steer_cmd < -100.) steer_cmd = -100.;
  int x = steer_cmd;
  uint8_t t = 0;

  t = x & 0xFF;
  Byte to_set0(data + 3);
  to_set0.set_value(t, 0, 8);
}

void Accelcmd130::set_p_option_cmd(uint8_t* data, uint32_t option_cmd) {
  if (option_cmd != 0x00 && option_cmd != 0x01) option_cmd = 0x01;

  uint8_t t = 0;

  t = option_cmd & 0xFF;
  Byte to_set0(data + 0);
  to_set0.set_value(t, 0, 8);
}

void Accelcmd130::calc_p_checksum(uint8_t* data) {
  uint8_t checksum = 0x00;
  checksum = (uint8_t)(ID & 0x00ff) + (uint8_t)(ID >> 8) + 0x08;
  for (uint8_t i = 0; i < (0x08 - 1); ++i)
    checksum += data[i];

  Byte to_set0(data + 7);
  to_set0.set_value(checksum, 0, 8);
}

void Accelcmd130::set_p_loop_cmd(uint8_t* data, uint32_t loop_cmd) {
  uint8_t t = 0;

  t = loop_cmd & 0xFF;
  Byte to_set0(data + 6);
  to_set0.set_value(t, 0, 8);
}

}  // namespace agilex
}  // namespace canbus
}  // namespace apollo
